﻿using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Text;

namespace ROVController
{
	public class PacketAPI
	{
		//constants
		private const int MAX_DATA_LENGTH = 0xFF;
		private const int MAX_PACKET_SIZE = MAX_DATA_LENGTH + 7;
		private const byte PACKET_START1 = 0xA5;
		private const byte PACKET_START2 = 0x5A;

		//member variables
		private byte sequence = 0;
		private byte[] packetBuffer = new byte[MAX_PACKET_SIZE];
		SerialPort serialPort;

		public PacketAPI(SerialPort serialPort)
		{
			this.serialPort = serialPort;
		}

		/// <summary>
		/// Sends a command packet with no data.
		/// </summary>
		/// <param name="packetType">The type of packet.</param>
		/// <returns>The sequence number that the packet was sent with.</returns>
		public byte sendPacket(PacketType packetType)
		{
			return sendPacket(packetType, new byte[0]);
		}

		/// <summary>
		/// Sends a command packet with data.
		/// </summary>
		/// <param name="packetType">The type of packet.</param>
		/// <param name="data">The data bytes. Length can be 0 to MAX_DATA_LENGTH.</param>
		/// <returns>The sequence number that the packet was sent with.</returns>
		public byte sendPacket(PacketType packetType, byte[] data)
		{
			if (data.Length > MAX_DATA_LENGTH)
			{
				throw new ArgumentException("data array cannot be longer than " + MAX_DATA_LENGTH, "data");
			}

			//packet start indicator bytes - allows the receiver to detect the beginning of a packet.
			packetBuffer[0] = PACKET_START1;
			packetBuffer[1] = PACKET_START2;
			//packet type - identifies the format of this packet.
			packetBuffer[2] = (byte)packetType;
			//sequence number - allows the receiver to detect if it misses a packet(s).
			packetBuffer[3] = sequence++;
			//data length - specifies the size of the payload to allow for variable-length packets.
			packetBuffer[4] = (byte)data.Length;
			//data bytes - the packet's payload
			Array.Copy(data, 0, packetBuffer, 5, data.Length);
			//16-bit checksum - allows the receiver to verify that the packet is most likely not corrupted.
			ushort checksum = calculateChecksum(packetBuffer);
			//pack checksum in "network order", MSB first.
			packetBuffer[5 + data.Length] = (byte)(checksum >> 8);
			packetBuffer[6 + data.Length] = (byte)(checksum & 0xFF);
			//transmit the data out the serial port
			serialPort.Write(packetBuffer, 0, 7 + data.Length);
			//return the sequence number
			return packetBuffer[3];
		}

		public ushort calculateChecksum(byte[] packet)
		{
			byte length = packetBuffer[4];
			ushort checksum = 0;
			for (int i = 2; i < length + 4; i++)
			{
				checksum += packet[i];
			}
			return checksum;
		}
	}
}
